﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using RosSharp;
using RosSharp.wing_msg;

/// <summary>
/// 为了使用Subscribe，必须安装Nuget包System.Reactive
/// </summary>

namespace RosNode
{
    /// <summary>
    /// 单例
    /// </summary>
    public class WingNode : RosNode
    {
        private static WingNode _node = null;

        static WingNode()
        {
            if(_node is null)
                _node = new WingNode();
        }

        private WingNode() { }

        public static WingNode CreateInstance()
        {
            return _node;
        }

        #region "Subscribers"
        private RosSharp.Topic.Subscriber<DriverState> drver_state_sub;
        private RosSharp.Topic.Subscriber<PoseCalibration> pose_calibrate_sub_front;
        #endregion

        #region "Publishers"
        private RosSharp.Topic.Publisher<RosSharp.std_msgs.String> test_pub;
        private RosSharp.Topic.Publisher<RosSharp.std_msgs.String> clear_odom_pub;
        private RosSharp.Topic.Publisher<RosSharp.std_msgs.String> abort_move_pub;
        private RosSharp.Topic.Publisher<RosSharp.std_msgs.Bool> toggle_pid_pub;
        private RosSharp.Topic.Publisher<RosSharp.std_msgs.String> arm_pub;
        private RosSharp.Topic.Publisher<RosSharp.std_msgs.String> raise_pub;
        private RosSharp.Topic.Publisher<RosSharp.std_msgs.String> update_pose_pub;
        private RosSharp.Topic.Publisher<OdomTf> set_odom_pub;
        private RosSharp.Topic.Publisher<MoveTask> move_task_pub;
        private RosSharp.Topic.Publisher<SetRunMode> run_mode_pub;
        #endregion

        public DriverState driver_state = new DriverState();
        public PoseCalibration pose_calibrate_front = new PoseCalibration() { errorinfo = "not ready" };

        private Dictionary<uint, string> TeamRoleDict = new Dictionary<uint, string>()
        {
            {0, "UnKown" },
            {1, "Master" },
            {2, "Slave" }
        };
        public readonly Dictionary<uint, string> RunModeDict = new Dictionary<uint, string>()
        {
            {0, "Separate" },
            {1, "Merge" },
            {2, "Parking" },
            {3, "Dismiss" },
            {4, "Offline" }
        };
        private Dictionary<uint, string> CtrlModeDict = new Dictionary<uint, string>()
        {
            {0, "Moving" },
            {1, "Spinning" },
            {2, "Shifting" },
            {3, "Begin spin"},
            {4, "Begin move"},
            {5, "Begin shift"}
        };
        private Dictionary<uint, string> ArmStateDict = new Dictionary<uint, string>()
        {
            {0, "UnKown" },
            {1, "Open" },
            {2, "Openning" },
            {3, "Closed"},
            {4, "Closing"}
        };
        private Dictionary<uint, string> RaiseStateDict = new Dictionary<uint, string>()
        {
            {0, "UnKown" },
            {1, "Up" },
            {2, "Moving Up" },
            {3, "Down"},
            {4, "Moving Down"},
        };

        public uint GetRunMode(string m)
        {
            var a = (from d in RunModeDict where d.Value == m select d.Key).ToList();
            if (a.Count > 0)
                return a[0];
            else
                return 99;
        }

        public string TeamRole { get => TeamRoleDict[driver_state.TeamRole]; }
        public string CtrlMode { get => CtrlModeDict[driver_state.CtrlMode]; }
        public string RunMode { get => RunModeDict[driver_state.RunMode]; }
        public string ArmState { get => ArmStateDict[driver_state.ArmState]; }
        public string RaiseState { get => RaiseStateDict[driver_state.RaiseState]; }

        public override void RegisterSubs()
        {
            drver_state_sub = n.SubscriberAsync<DriverState>("/driver_state").Result;
            pose_calibrate_sub_front = n.SubscriberAsync<PoseCalibration>("/pose_calibrate_front").Result;
            drver_state_sub.Subscribe(OnDriverState);
            pose_calibrate_sub_front.Subscribe(e => pose_calibrate_front = e);
            Console.WriteLine("Registing");
        }

        public override void RegisterPubs()
        {
            test_pub = n.PublisherAsync<RosSharp.std_msgs.String>("/dotnet_test").Result;
            arm_pub = n.PublisherAsync<RosSharp.std_msgs.String>("/set_arm").Result;
            raise_pub = n.PublisherAsync<RosSharp.std_msgs.String>("/set_raise").Result;
            clear_odom_pub = n.PublisherAsync<RosSharp.std_msgs.String>("/clear_odom").Result;
            abort_move_pub = n.PublisherAsync<RosSharp.std_msgs.String>("/abort_move").Result;
            toggle_pid_pub = n.PublisherAsync<RosSharp.std_msgs.Bool>("/toggle_pid").Result;
            update_pose_pub = n.PublisherAsync<RosSharp.std_msgs.String>("/update_pose").Result;
            set_odom_pub = n.PublisherAsync<OdomTf>("/set_odom").Result;
            move_task_pub = n.PublisherAsync<MoveTask>("/move_task").Result;
            run_mode_pub = n.PublisherAsync<SetRunMode>("/run_mode").Result;
        }

        public override void DisposePubs()
        {
            base.DisposePubs();
            test_pub.Dispose();
        }

        public override void DisposeSubs()
        {
            base.DisposeSubs();
            drver_state_sub.Dispose();
        }

        public void PublishString(RosSharp.Topic.Publisher<RosSharp.std_msgs.String> pub, string msg)
        {
            if (!PubReady)
                return;
            pub.OnNext(new RosSharp.std_msgs.String() { data = msg });
        }

        public void PublishTestTopic(string msg)
        {
            PublishString(test_pub, msg);
        }

        public void UpdatePose()
        {
            PublishString(update_pose_pub, "null");
        }

        public void SetArm(string msg)
        {
            PublishString(arm_pub, msg);
        }

        public void SetRaise(string msg)
        {
            PublishString(raise_pub, msg);
        }

        public void PublishTogglePid(bool b)
        {
            if (!PubReady)
                return;
            toggle_pid_pub.OnNext(new RosSharp.std_msgs.Bool() { data = b });
        }

        public void SetOdom(double x, double y, double yaw)
        {
            if (!PubReady)
                return;
            set_odom_pub.OnNext(
                new OdomTf()
                {
                    dx = (float)x,
                    dy= (float)y,
                    dth = (float)yaw
                }
            );
        }

        public void SetRunMode(string m)
        {
            if (!PubReady)
                return;
            run_mode_pub.OnNext(
                new SetRunMode()
                {
                    TeamDistance = 2.5,
                    RunMode = GetRunMode(m)
                }
            );
        }

        public void SetYaw(int yaw)
        {
            if (!PubReady)
                return;
            move_task_pub.OnNext(
                new MoveTask()
                    {
                        task_id = 999,
                        goTh = yaw,
                        bodyTh = yaw,
                        x = 0,
                        y = 0,
                        distance = 0
                    }
                );
        }

        public void MoveDistance(bool manually, double distance, double target_x, double target_y)
        {
            if (!PubReady)
                return;

            double x = driver_state.Lx + distance * Math.Cos(driver_state.KeepYaw/180.0*Math.PI);
            double y = driver_state.Ly + distance * Math.Sin(driver_state.KeepYaw/180.0*Math.PI);

            if(manually)
            {
                x = target_x;
                y = target_y;
                distance = 1.0;
            }

            move_task_pub.OnNext(
                new MoveTask()
                {
                    task_id = 999,
                    goTh = driver_state.KeepYaw,
                    bodyTh = driver_state.KeepYaw,
                    x = x,
                    y = y,
                    distance = distance
                }
                );
        }

        public void ClearOdom()
        {
            if (!PubReady)
                return;
            clear_odom_pub.OnNext(new RosSharp.std_msgs.String() { data = "clear" });
        }

        public void AbortMove()
        {
            if (!PubReady)
                return;
            abort_move_pub.OnNext(new RosSharp.std_msgs.String() { data = "abort" });
        }

        private void OnDriverState(DriverState e)
        {
            driver_state = e;
        }
    }
}
